/**
  ******************************************************************************
  * @文件名           : fml_pid.c
  * @作者             : citunhk
  * @功能介绍         : PID算法
  *	@日期             : 2022.11.15
  ******************************************************************************
  * @注意
  *
  *
  *
  ******************************************************************************
  *更改记录
  *日期 修改人 修改内容
  *
  *
  *
  ******************************************************************************
  */
#include "fml_pid.h"
#include "dri_flash_sst.h"
#include "dri_DigitalQuantity.h"
#include "sys_data.h"

PID_Type PID;

float gP,gI,gD,gPID;

/**
* @函数名	FML_PID_Restoru
* @功能	    按出厂参数重置PID参数
* @入口参数	无
* @返回值	无
*/
void FML_PID_Restoru(void)
{
	//温度PID
	PID.Per[level_01].TKP = 8;
	PID.Per[level_01].TT  = 10;
	PID.Per[level_01].TTI = 100;
	PID.Per[level_01].TTD = 60;
	PID.Per[level_01].TKI = PID.Per[level_01].TKP * (PID.Per[level_01].TT / PID.Per[level_01].TTI);
	PID.Per[level_01].TKD = PID.Per[level_01].TKP * (PID.Per[level_01].TTD / PID.Per[level_01].TT);
	PID.Per[level_01].TErr = PID.Per[level_01].TErr1 = PID.Per[level_01].TErr2 = 0;

	PID.Per[level_02].TKP = 0.32;
	PID.Per[level_02].TT  = 5.0;
	PID.Per[level_02].TTI = 300.0;
	PID.Per[level_02].TTD = 15.0;
	PID.Per[level_02].TKI = PID.Per[level_02].TKP * (PID.Per[level_02].TT / PID.Per[level_02].TTI);
	PID.Per[level_02].TKD = PID.Per[level_02].TKP * (PID.Per[level_02].TTD / PID.Per[level_02].TT);
	PID.Per[level_02].TErr = PID.Per[level_02].TErr1 = PID.Per[level_02].TErr2 = 0;

	PID.Per[level_03].TKP = 0.33;
	PID.Per[level_03].TT  = 5.0;
	PID.Per[level_03].TTI = 310.0;
	PID.Per[level_03].TTD = 15.0;
	PID.Per[level_03].TKI = PID.Per[level_03].TKP * (PID.Per[level_03].TT / PID.Per[level_03].TTI);
	PID.Per[level_03].TKD = PID.Per[level_03].TKP * (PID.Per[level_03].TTD / PID.Per[level_03].TT);
	PID.Per[level_03].TErr = PID.Per[level_03].TErr1 = PID.Per[level_03].TErr2 = 0;

	//流量PID
	PID.Per[0].FErr = PID.Per[0].FErr1 = PID.Per[0].FErr2 = 0;
}

/**
* @函数名	FML_PIDParameterDeinit
* @功能	    按出厂参数重置PID参数
* @入口参数	无
* @返回值	无
*/
void FML_PIDParameterDeinit(void)
{
	//温度PID
	PID.Per[level_01].TKP = UP.Para.TempPID_P1;
	PID.Per[level_01].TT  = 10;
	PID.Per[level_01].TTI = UP.Para.TempPID_I1;
	PID.Per[level_01].TTD = UP.Para.TempPID_D1;
	PID.Per[level_01].TKI = PID.Per[level_01].TKP * (PID.Per[level_01].TT / PID.Per[level_01].TTI);
	PID.Per[level_01].TKD = PID.Per[level_01].TKP * (PID.Per[level_01].TTD / PID.Per[level_01].TT);
	PID.Per[level_01].TErr = PID.Per[level_01].TErr1 = PID.Per[level_01].TErr2 = 0;

	PID.Per[level_02].TKP = UP.Para.TempPID_P;
	PID.Per[level_02].TT  = 5.0;
	PID.Per[level_02].TTI = UP.Para.TempPID_I;
	PID.Per[level_02].TTD = UP.Para.TempPID_D;
	PID.Per[level_02].TKI = PID.Per[level_02].TKP * (PID.Per[level_02].TT / PID.Per[level_02].TTI);
	PID.Per[level_02].TKD = PID.Per[level_02].TKP * (PID.Per[level_02].TTD / PID.Per[level_02].TT);
	PID.Per[level_02].TErr = PID.Per[level_02].TErr1 = PID.Per[level_02].TErr2 = 0;

	PID.Per[level_03].TKP = UP.Para.TempPID_P3;
	PID.Per[level_03].TT  = 5.0;
	PID.Per[level_03].TTI = UP.Para.TempPID_I3;
	PID.Per[level_03].TTD = UP.Para.TempPID_D3;
	PID.Per[level_03].TKI = PID.Per[level_03].TKP * (PID.Per[level_03].TT / PID.Per[level_03].TTI);
	PID.Per[level_03].TKD = PID.Per[level_03].TKP * (PID.Per[level_03].TTD / PID.Per[level_03].TT);
	PID.Per[level_03].TErr = PID.Per[level_03].TErr1 = PID.Per[level_03].TErr2 = 0;

}

/**
* @函数名	FML_PID_TempOperation
* @功能	    温度PID计算
* @入口参数	无
* @返回值	f_PWM_out 输出PWM值
*/
float FML_PID_TempOperation(void)
{
	float f_increment;					//增量式PID计算结果
	static float f_PWM_out;						//PWM输出值
	static uint8_t pidLv;						//PID温度分区号
	static uint8_t oldPidLv = 10;		//旧的PID温度分区号
	static float nowSetTemp, lastSetTemp;

//	FML_PIDParameterDeinit();  //刷新PID参数

	/* 首先根据设定温度确定PID参数分区号 */
	nowSetTemp = gSV_Temp1;
	if (nowSetTemp <= 5)
		pidLv = level_01;
	else if (nowSetTemp <= 45)
		pidLv = level_02;
	else
		pidLv = level_03;

	PID.Per[level_01].TKP = UP.Para.TempPID_P1;
	PID.Per[level_01].TT  = 10;
	PID.Per[level_01].TTI = UP.Para.TempPID_I1;
	PID.Per[level_01].TTD = UP.Para.TempPID_D1;
	PID.Per[level_01].TKI = PID.Per[level_01].TKP * (PID.Per[level_01].TT / PID.Per[level_01].TTI);
	PID.Per[level_01].TKD = PID.Per[level_01].TKP * (PID.Per[level_01].TTD / PID.Per[level_01].TT);

	PID.Per[level_02].TKP = UP.Para.TempPID_P;
	PID.Per[level_02].TT  = 10;
	PID.Per[level_02].TTI = UP.Para.TempPID_I;
	PID.Per[level_02].TTD = UP.Para.TempPID_D;
	PID.Per[level_02].TKI = PID.Per[level_02].TKP * (PID.Per[level_02].TT / PID.Per[level_02].TTI);
	PID.Per[level_02].TKD = PID.Per[level_02].TKP * (PID.Per[level_02].TTD / PID.Per[level_02].TT);

	PID.Per[level_03].TKP = UP.Para.TempPID_P3;
	PID.Per[level_03].TT  = 10;
	PID.Per[level_03].TTI = UP.Para.TempPID_I3;
	PID.Per[level_03].TTD = UP.Para.TempPID_D3;
	PID.Per[level_03].TKI = PID.Per[level_03].TKP * (PID.Per[level_03].TT / PID.Per[level_03].TTI);
	PID.Per[level_03].TKD = PID.Per[level_03].TKP * (PID.Per[level_03].TTD / PID.Per[level_03].TT);

//	PID.Per[pidLv].TErr = nowSetTemp - gPV_Temp1;	// 偏差值 = 设定值 - 测量值
	PID.Per[pidLv].TErr = nowSetTemp - gPV_TempPID1;

	/* 然后给PID参数赋初值 */
	if (pidLv != oldPidLv)//PID区域发生改变
	{
		PID.Per[pidLv].TErr2 = PID.Per[pidLv].TErr1 = PID.Per[pidLv].TErr;
		oldPidLv = pidLv;
	}

	/* 最后进行PID计算 */
	if (lastSetTemp != nowSetTemp)										//如果检测改变了设定温度
	{
		PID.Per[pidLv].TErr2 = PID.Per[pidLv].TErr1 = PID.Per[pidLv].TErr;//偏差值重新赋值
		lastSetTemp = nowSetTemp;
	}

	f_increment = PID.Per[pidLv].TKP * ( PID.Per[pidLv].TErr - PID.Per[pidLv].TErr1)
							+ PID.Per[pidLv].TKI * PID.Per[pidLv].TErr
							+ PID.Per[pidLv].TKD * ( PID.Per[pidLv].TErr - 2 * PID.Per[pidLv].TErr1 + PID.Per[pidLv].TErr2 );


	gP = PID.Per[pidLv].TKP * ( PID.Per[pidLv].TErr - PID.Per[pidLv].TErr1);
	gI = PID.Per[pidLv].TKI * PID.Per[pidLv].TErr;
	gD = PID.Per[pidLv].TKD * ( PID.Per[pidLv].TErr - 2 * PID.Per[pidLv].TErr1 + PID.Per[pidLv].TErr2 );
	gPID = f_increment;

	PID.Per[pidLv].TErr2 = PID.Per[pidLv].TErr1;
	PID.Per[pidLv].TErr1 = PID.Per[pidLv].TErr;

	f_PWM_out += f_increment;

	/* PWM数据超限处理 PWM输出值范围为 0至100*/
	if ( f_PWM_out > 100.0 )
		f_PWM_out = 100.0;
	else if ( f_PWM_out < 0.0 )
		f_PWM_out = 0.0;

	return f_PWM_out;
}

/**
* @函数名	FML_PWM_Out
* @功能	    PWM输出程序，由于控制的为固态继电器驱动的加热管，频率要求不宜过高
* @入口参数	无
* @返回值	无
*/
void FML_PWM_Out (void)
{
	static uint8_t PWMTimes = 1;
	static uint8_t PIDcycle = 0;
	static uint8_t PWM_out;

	if ( PWMTimes <= PWM_out )
		DRI_EQU_SetFunState(HEAT,ON);		//开启加热
	else
		DRI_EQU_SetFunState(HEAT,OFF);	//关闭加热
	if( ++PWMTimes > 100)
	{
		PWMTimes = 1;
		/* 5个周期 进行一次PID计算 */
		if ( PIDcycle == 4 )
		{
			PWM_out = (uint8_t)FML_PID_TempOperation();
			PIDcycle = 0;
		}
		else
			PIDcycle++;
	}
}

/**
* @函数名	FML_PID_FlowOperation
* @功能	    流量PID计算
* @入口参数	无
* @返回值	f_PWM_out 输出PWM值
*/
float FML_PID_FlowOperation(void)
{
	float f_increment;					//增量式PID计算结果
	static float f_PWM_out;						//PWM输出值
	static uint8_t pidLv;						//PID温度分区号
	static uint8_t oldPidLv = 10;		//旧的PID流量分区号
	static float nowSetTemp, lastSetTemp;

	/* 首先根据设定流量确定PID参数分区号 */
	nowSetTemp = UP.Para.SV_Flow;

//	if(((nowSetTemp - gPV_Flow) > 3) || ((gPV_Flow - nowSetTemp) > 3))
//	{
//		PID.Per[0].FKP = 5;
//		PID.Per[0].FT  = 5.0;
//		PID.Per[0].FTI = 100;
//		PID.Per[0].FTD = 0.5;
//		PID.Per[0].FKI = PID.Per[0].FKP * (PID.Per[0].FT / PID.Per[0].FTI);
//		PID.Per[0].FKD = PID.Per[0].FKP * (PID.Per[0].FTD / PID.Per[0].FT);
//
//		pidLv = 0;
//	}
//	else
//	{
		PID.Per[1].FKP = UP.Para.FlowPID_P;
		PID.Per[1].FT  = 5.0;
		PID.Per[1].FTI = UP.Para.FlowPID_I;
		PID.Per[1].FTD = UP.Para.FlowPID_D;
		PID.Per[1].FKI = PID.Per[1].FKP * (PID.Per[1].FT / PID.Per[1].FTI);
		PID.Per[1].FKD = PID.Per[1].FKP * (PID.Per[1].FTD / PID.Per[1].FT);

		pidLv = 1;
//	}

	/* 最后进行PID计算 */
	PID.Per[pidLv].FErr = nowSetTemp - gPV_Flow;	// 偏差值 = 设定值 - 测量值

	/* 然后给PID参数赋初值 */
	if (pidLv != oldPidLv)//PID区域发生改变
	{
		PID.Per[pidLv].FErr2 = PID.Per[pidLv].FErr1 = PID.Per[pidLv].FErr;
		oldPidLv = pidLv;
	}

//	if((PID.Per[pidLv].FErr > -2) && (PID.Per[pidLv].FErr < 2))
//	{
//		if(PID.Per[pidLv].FErr )
		if (lastSetTemp != nowSetTemp)										//如果检测改变了设定温度
		{
			PID.Per[pidLv].FErr2 = PID.Per[pidLv].FErr1 = PID.Per[pidLv].FErr;//偏差值重新赋值
			lastSetTemp = nowSetTemp;
		}
		f_increment = PID.Per[pidLv].FKP * ( PID.Per[pidLv].FErr - PID.Per[pidLv].FErr1)
								+ PID.Per[pidLv].FKI * PID.Per[pidLv].FErr
								+ PID.Per[pidLv].FKD * ( PID.Per[pidLv].FErr - 2 * PID.Per[pidLv].FErr1 + PID.Per[pidLv].FErr2 );
		PID.Per[pidLv].FErr2 = PID.Per[pidLv].FErr1;
		PID.Per[pidLv].FErr1 = PID.Per[pidLv].FErr;

		f_PWM_out += f_increment;

		/* PWM数据超限处理 PWM输出值范围为 0至100*/
		if ( f_PWM_out > 100.0 )
			f_PWM_out = 100.0;
		else if ( f_PWM_out < 0.0 )
			f_PWM_out = 0.0;
//	}
//	else if(PID.Per[pidLv].FErr <= -2)
//	{
//		f_PWM_out = 100;
//	}
//	else if(PID.Per[pidLv].FErr >= 2)
//	{
//		f_PWM_out = 0;
//	}
//
	return f_PWM_out;
}

/**
* @函数名	FML_PID_PressOperation
* @功能	    压力PID计算
* @入口参数	无
* @返回值	f_PWM_out 输出PWM值
*/
float FML_PID_PressOperation(void)
{
	float f_increment;					//增量式PID计算结果
	static float f_PWM_out;						//PWM输出值
	static uint8_t pidLv;						//PID温度分区号
	static uint8_t oldPidLv = 10;		//旧的PID流量分区号
	static float nowSetPress, lastSetPress;

	/* 首先根据设定压力确定PID参数分区号 */

	if (nowSetPress <= 1000)
		pidLv = level_01;
	else if (nowSetPress <= 2000)
		pidLv = level_02;
	else
		pidLv = level_03;

	nowSetPress = UP.Para.SV_Press;

	PID.Per[0].FKP = UP.Para.FlowPID_P;
	PID.Per[0].FT  = 5.0;
	PID.Per[0].FTI = UP.Para.FlowPID_I;
	PID.Per[0].FTD = UP.Para.FlowPID_D;
	PID.Per[0].FKI = PID.Per[0].FKP * (PID.Per[0].FT / PID.Per[0].FTI);
	PID.Per[0].FKD = PID.Per[0].FKP * (PID.Per[0].FTD / PID.Per[0].FT);


	/* 最后进行PID计算 */
	PID.Per[pidLv].FErr = nowSetPress - gPV_Press_Out;	// 偏差值 = 设定值 - 测量值

	/* 然后给PID参数赋初值 */
	if (pidLv != oldPidLv)//PID区域发生改变
	{
		PID.Per[pidLv].FErr2 = PID.Per[pidLv].FErr1 = PID.Per[pidLv].FErr;
		oldPidLv = pidLv;
	}

	if (lastSetPress != nowSetPress)										//如果检测改变了设定温度
	{
		PID.Per[pidLv].FErr2 = PID.Per[pidLv].FErr1 = PID.Per[pidLv].FErr;//偏差值重新赋值
		lastSetPress = nowSetPress;
	}
	f_increment = PID.Per[pidLv].FKP * ( PID.Per[pidLv].FErr - PID.Per[pidLv].FErr1)
							+ PID.Per[pidLv].FKI * PID.Per[pidLv].FErr
							+ PID.Per[pidLv].FKD * ( PID.Per[pidLv].FErr - 2 * PID.Per[pidLv].FErr1 + PID.Per[pidLv].FErr2 );
	PID.Per[pidLv].FErr2 = PID.Per[pidLv].FErr1;
	PID.Per[pidLv].FErr1 = PID.Per[pidLv].FErr;

	f_PWM_out += f_increment;

	/* PWM数据超限处理 PWM输出值范围为 0至100*/
	if ( f_PWM_out > 100.0 )
		f_PWM_out = 100.0;
	else if ( f_PWM_out < 0.0 )
		f_PWM_out = 0.0;

	return f_PWM_out;
}

